﻿/*
 * Copyright 2023 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : nearest_search.h
 *@brief   : nearest point search
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2023/10/18
 *
 */
#ifndef NEAREST_SEARCH_H
#define NEAREST_SEARCH_H

#include "mapping/distance_map.h"

#include "mapping/grid_2d.h"
#include "sensor/point_cloud.h"

namespace cartographer {
namespace scan_matching {

using namespace mapping;

class NearestSearch {
public:  
  NearestSearch();
  NearestSearch(const Grid2D& grid);

  ~NearestSearch();

  void BuildMapCSpace(const Grid2D& grid);
  void ComputeJacobian(const transform::Rigid2d& pose_estimate_2d, const sensor::PointCloud& point_cloud, std::vector<Eigen::Matrix<float, 2, 3>>& jacobians);
  void ComputeResidual(const sensor::PointCloud& reduced_cloud_src, const sensor::PointCloud& reduced_cloud_tgt, std::vector<Eigen::Vector2f>& residuals);

  std::vector<Eigen::Array2i> GetMaxProbabilityPoint(const sensor::PointCloud& cloud, const transform::Rigid2d& initial_pose_estimate, const Grid2D& grid, const bool& is_use_best_slot);
  void BuildMapCSpaceByMaxProbability(const sensor::PointCloud& cloud, const transform::Rigid2d& initial_pose_estimate, const Grid2D& grid, const bool& is_use_best_slot = false);
  
  float ComputeProbBilinearInterpolate(const Eigen::Array2i& cell_idx) const;
  float ComputeProbabilityByGaussian(const float& distance) const;
private:
  DisMap *dist_map_ptr_;
};

} // namespace scan_matching
}

#endif
